import math
import numpy as np

import pat.vehicles.rotorcraft.dynamic_model_planar_simple as dm



def state_and_input_from_flat_out(Y, P):
    X = np.zeros(dm.sv_size)
    X[dm.sv_x] = Y[0, 0]
    X[dm.sv_z] = Y[1, 0]
    x2 = Y[0, 2]
    z2pg = Y[1, 2]+9.81
    X[dm.sv_th] = -math.atan2(x2, z2pg)
    
    X[dm.sv_xd] = Y[0, 1]
    X[dm.sv_zd] = Y[1, 1]
    x3 =  Y[0,3]
    z3 =  Y[1,3]
    X[dm.sv_thd] = -(z2pg*x3-x2*z3)/(z2pg**2+x2**2)

    ut = P.m*math.sqrt(x2**2 + z2pg**2)
    x4 = Y[0,4]
    z4 = Y[1,4]
    a = x4*z2pg - z4*x2
    b = x2**2 + z2pg**2
    c = 2*(z2pg*z3 + x2*x3)
    d = x3*z2pg-z3*x2
    ud = -P.J/P.l*(a/b - c*d/b**2)
    U = [(ut+ud)/2., (ut-ud)/2.]

    return X, U

